#ifndef __VECTOR4_H
#define __VECTOR4_H

#include <cmath>
#include "Vector3.h"
#include <iostream>

class Vector4 {
 public:
	double v[4];

	Vector4() {
		for (int i=0; i<4; i++) {
			v[i] = 0;
		}		
	}

	Vector4(Vector3 vec, double w) {
		v[0] = vec[0];
		v[1] = vec[1];
		v[2] = vec[2];
		v[3] = w;
	}

	Vector4(double x, double y, double z, double w) {
		v[0] = x;
		v[1] = y;
		v[2] = z;
		v[3] = w;
	}

	void set(double x, double y, double z, double w) {
		v[0] = x;
		v[1] = y;
		v[2] = z;
		v[3] = w;
	}
	
	double get(int i) const {
		return v[i];
	}

	const double & operator[] (int i) const
	{
		return v[i];
	}
 
	void addInPlace(Vector4 &a) {
		for (int i=0; i<4; i++) {
			v[i] += a.v[i];
		}
	}

	static Vector4 add(Vector4 &a, Vector4 &b) {
		Vector4 result(0,0,0,0);
		result.addInPlace(a);
		result.addInPlace(b);
		return result;
	}

	Vector4 operator+(const Vector4 &rhs) const
	{
		return Vector4(v[0] + rhs.v[0], v[1] + rhs.v[1], v[2] + rhs.v[2], v[3] + rhs.v[3]);
	}

	Vector4 operator-(const Vector4 &rhs) const
	{
		return Vector4(v[0] - rhs.v[0], v[1] - rhs.v[1], v[2] - rhs.v[2], v[3] - rhs.v[3]);
	}
	
	Vector4 operator*(const double &rhs) const
	{
		return Vector4(rhs*v[0], rhs*v[1], rhs*v[2], rhs*v[3]);
	}

	double dot(const Vector4& lhs, const Vector4& rhs) const
	{
		return    lhs.v[0] * rhs.v[0] 
	        	+ lhs.v[1] * rhs.v[1] 
			+ lhs.v[2] * rhs.v[2];
	}

	void dehomogenize() {
		for (int i=0; i<3; i++) {
			v[i] = v[i] / v[3];
		}

		v[3] = 1;
	}

	void print() {
		std::cout << "[";
		for (int i=0; i<3; i++)
			std::cout << v[i] << ", ";
		std::cout << v[3] << "]" << std::endl;
	}
};



Vector4 operator*(const double &lhs, const Vector4 &rhs)
{
	return rhs*lhs;
}

#endif
